package com.rexense.beacon.alg;

import Jama.Matrix;
import com.rexense.beacon.entity.Beacon;
import com.rexense.beacon.entity.Location;
import com.rexense.beacon.entity.Msg;

import java.util.Comparator;
import java.util.List;
import java.util.Map;
import java.util.stream.Collectors;

public final class Trilateral {

    /**
     * 定位
     *
     * @param msgList   消息列表,不存在相同beaconId的消息
     * @param beaconMap key->beaconId, value->beacon
     * @return Location, 不能定位返回null, 若msgList.size < 3抛出异常
     */
    public static Location locate(List<Msg> msgList, Map<String, Beacon> beaconMap) {
        if (msgList == null || msgList.size() < 3) {
            throw new IllegalArgumentException("参数不合法，msgList.size >= 3");
        }

        msgList.sort(Comparator.comparing(Msg::getRssi).reversed());
        List<Msg> useList = msgList.subList(0, 3);

        List<BleBase> baseList = useList.stream().map(msg -> {
            Beacon beacon = beaconMap.get(msg.getBeaconId());
            return BleBase.builder()
                    .beaconId(msg.getBeaconId())
                    .rssi(msg.getRssi())
                    .h(beacon.getH())
                    .n(beacon.getN())
                    .p0(beacon.getP0())
                    .x(beacon.getX())
                    .y(beacon.getY())
                    .build()
                    .distinct();
        }).collect(Collectors.toList());


        return locate(baseList);
    }

    /**
     * 多点定位，不只是三点
     *
     * @param baseList 信标距离数据，三条数据以上
     * @return 位置
     */
    public static Location locate(List<BleBase> baseList) {
        int len = baseList.size();

        double[][] a = new double[len - 1][2];
        double[][] b = new double[len - 1][1];

        BleBase bleBaseN = baseList.get(len - 1);
        for (int i = 0; i < len - 1; i++) {
            BleBase bleBaseI = baseList.get(i);
            a[i][0] = 2 * (bleBaseI.getX() - bleBaseN.getX());
            a[i][1] = 2 * (bleBaseI.getY() - bleBaseN.getY());

            b[i][0] = Math.pow(bleBaseI.getX(), 2) - Math.pow(bleBaseN.getX(), 2)
                    + Math.pow(bleBaseI.getY(), 2) - Math.pow(bleBaseN.getY(), 2)
                    + Math.pow(bleBaseN.getDistinct(), 2) - Math.pow(bleBaseI.getDistinct(), 2);
        }

        Matrix matrixA = new Matrix(a);
        Matrix matrixB = new Matrix(b);

        Matrix matrixAT = matrixA.transpose();

        Matrix timesAtA = matrixAT.times(matrixA);

//        while (timesAtA.det() == 0.) {
//            double[][] array = timesAtA.getArray();
//            for (int i = 0; i < array.length; i++) {
//                for (int j = 0; j < array[0].length; j++) {
//                    if (array[i][j] == 0.) {
//                        array[i][j] += 0.0000000001;
//                    }
//                }
//            }
//        }
        Matrix timesAtAInverse = timesAtA.inverse();

        Matrix timesAtAInverseAT = timesAtAInverse.times(matrixAT);

        Matrix timesAtAInverseATB = timesAtAInverseAT.times(matrixB);

        double[][] array = timesAtAInverseATB.getArray();

        return new Location(array[0][0], array[1][0]);
    }
}
